/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

// File: serialTest.cpp
// Author: Cory Dixon
// Date: 2/27/01
//
// Program to test the serial port both in Unix
// and VxWorks
//

#include <unistd.h>
#include <signal.h>
#include <iostream.h>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <sys/types.h>
#include <fcntl.h>

#include "String.h"
#include "serial.h"
#include "timeLib.h"
#include "command.h"
#include "queue.h"
#include "type_defs.h"

// functions
void usage();		  // prints usage description
void exitsignal( int signal );   // catches and deals with exiting signals
void myclose(int value);    // closes open fd and exits

void clientMode();
void hostMode();
void closeConnection() ;
void parseClientCommand(Command &cmd);
void parseHostCommand(Command &cmd);


#define MAXLINE 256

// global data
SerialPort *serial;

//
// Global Data
//
TimeStamp tsTx;
TimeStamp tsRx;
long long totalTime = 0;
unsigned long int minTime   = 999999999;
unsigned long int maxTime   = 0;
bool Connected = false;
bool sent = false;
volatile static bool hostRun = TRUE;
int waitCount;

#define NUM_LOOP	1024
Queue timeQueue(NUM_LOOP);

enum Mode {HOST, CLIENT};

//----------------------------------------------------------
// Main function
//----------------------------------------------------------

#ifdef VXWORKS
int serialTest(char *dev, char *baud, int runMode)
#else
int main(int argc, char *argv[])
#endif
{
	Mode mode = CLIENT;
	String devStr = "/dev/ttyS0";
	String baudStr = "9600";

#ifndef VXWORKS
	// used for getopt to get otions from arguments
	extern int optind;
	//extern char * optarg;

	int c;
	while ((c=getopt(argc,argv,"hc")) != EOF) switch( (char)c)
	   {
		case 'h':
			mode = HOST;
			break;
		case 'c':
			mode = CLIENT;
			break;
		default:
			cout << "Unknown arg " << argv[optind - 1] << endl;
			usage();
			exit(EXIT_SUCCESS);
	}

	if( optind + 2  ==  argc) {
		devStr = String(argv[optind]); 
		baudStr = String(argv[optind + 1]);	
	} else {
		usage();
		exit(EXIT_SUCCESS);
	}
#else
	if( dev != NULL )
		devStr = String( dev );
	if( baud != NULL )
		baudStr = String( baud );

	if( runMode == 1 )
		mode = HOST;
	else
		mode = CLIENT;
#endif

	 // catch signals
	signal( SIGINT, exitsignal);
	//signal( SIGQUIT, exitsignal);
	//signal( SIGTERM, exitsignal);

	// open serial port
	serial = new SerialPort(devStr.c_str(),baudStr.c_str());
	assert(serial != NULL);

	if( serial->init() != SerialPort::Connected ){
		printf("Could not open serial port\n");
		exit(EXIT_FAILURE);
	}

	// set serial to be blocking and raise control lines
	serial->setBlocking();
	serial->setLocal();

	// go into the appropriate mode
	// these don't return untill they are finished, 
	// i.e. the serial was closed
	if( mode == CLIENT )
		clientMode();
	else
		hostMode();

	// exit
	myclose(EXIT_SUCCESS);
}


//----------------------------------------------------------
// print usage
//----------------------------------------------------------
void usage()
{
	printf("usage: serialTest {-c | -h} dev baud \n");
}

//----------------------------------------------------------
// catch exit signals
//----------------------------------------------------------
void exitsignal( int signal )
{
	printf("Caught signal %d ... exiting\n",signal);
	fflush(stdout);
	
	hostRun = FALSE;
}

//----------------------------------------------------------
// close down properly
//----------------------------------------------------------
void myclose(int value)
{
	closeConnection();	 

	delete serial;

	exit(value);
}

//----------------------------------------------------------
// close connection
//----------------------------------------------------------
void closeConnection() 
{
	uint8_t cmdBuf[] = { CMD_DISCONNECTING, 0, 0, 0};
	Command cmd(cmdBuf);
	cmd.setCheckSum();

	cout << "Closing connection ... ";

	// send disconnect command to host
	if( serial != NULL) {
		if( serial->getStatus() == SerialPort::Connected ) 
			serial->write( cmd );
		else
			cout << "not connected ";
		
		// close serial
		serial->close();

		// print serialet statistics
		serial->printStats();
	}
	cout << "closed" << endl;
}


//----------------------------------------------------------
// Act on received commands
//----------------------------------------------------------
void parseClientCommand(Command &cmd)
{
	if( !cmd.isValid() ) {
		cout << "<!!! parseCommand - invalid checksum !!!>\n";
		cmd.printCmd();

		return;
	}	  

	switch( cmd.cmd() ) {
	// check if was host replying to connection request
	case CMD_CONNECTING:
		cout << "<--- Connected to host --->" << endl;
		Connected = true;
		break;

	// check if exiting
	case CMD_DISCONNECTING:
		cout << "<--- Reqeust from host to disconnect --->" << endl;
		Connected = false;
		break;

	// host returned the time command
	case CMD_TIME_LOOP:

		cout << "<--- Time-Loop --->" << endl << flush;

		// simply send it back ASAP
		if( serial->write(cmd) != cmd.size() ){
			cout << "<!!! Could not send back time-loop!!!>" <<endl; 
			perror("serial->write" );
			return;
		}
		break;

	default:
		//cout << "<!!! Unknown command " << (int)(cmd.cmd()) << " !!!>" << endl;
		break;
	}

}

void parseHostCommand(Command &cmd)
{
	if( !cmd.isValid() ) {
		cout << "<!!! parseCommand - invalid checksum !!!>\n";
		cmd.printCmd();

		return;
	}	  

	switch( cmd.cmd() ) {
	// check if connecting
	case CMD_CONNECTING:
	{
		cout << "<--- Request to connect --->" << endl << flush;

		uint8_t cmdBuf[] = {CMD_CONNECTING, 0, 0, 0};
		Command send(cmdBuf);
		send.setCheckSum();

		if( serial->write(send) != send.size() )
			cout << "!!! Could not connect client !!!" <<endl; 
		else {
			cout << "Client connected" << endl;
			Connected = true;
			sent = false;
		}
	}
		break;

	// check if exiting
	case CMD_DISCONNECTING:
		cout << "<--- Reqeust to disconnect --->" << endl;
		break;

	case CMD_TIME_LOOP:
	{
		unsigned long deltaT;
		register float msec;

		// calculate the time diff
		tsRx.stamp();
		deltaT = tsRx - tsTx;
		maxTime = (deltaT > maxTime ? deltaT : maxTime);
		minTime = (deltaT < minTime ? deltaT : minTime);
		totalTime += deltaT;
		msec = deltaT / 1000.0;
		timeQueue.Push( (QueueType *)&(msec) ); // save as milli-sec

		//cout << "ping returned" << endl;
		sent = false;
	}
		break;

	default:
		if( cmd.isDefined() ) {
			cout << "CMD: " << DefinedCommands[ cmd.cmd() ].name << endl;
		} else
			cout << "<!!! Unknown command " << (int)(cmd.cmd()) << " !!!>" << endl;
		break;
	}
}

//----------------------------------------------------------
// Run like a Client
//----------------------------------------------------------
void clientMode()
{
	uint8_t *rxBuf = new uint8_t[ CMD_MAX_SIZE ];
	int rxNum = 0;
	CommandQueue serialQueue;		// parse incoming serialet stream
	SerialPort::SerialWait serialWait;	// return value for wait

	// connect command
	uint8_t cntBuf[] = {CMD_CONNECTING, 0, 0, 0};
	Command cntCmd(cntBuf);
	cntCmd.setCheckSum();

	int i=0;
	while( i < NUM_LOOP) 
	{
		// try to connect to host
		if( Connected == false && serial->getStatus() == SerialPort::Connected ) {
			cout << "Trying to connect to host" << endl;

			// don't let output buffer build up
			serial->flushIO();

			// send to host
			if( serial->write(cntCmd) != cntCmd.size() ){
				cout << "!!! Could not send client connecting !!!" <<endl; 
				perror ("serial->write");
				break;
			}
		} 

		// wait 1000 milli-secs for reply
		serialWait = serial->wait(1000); // wait

		if( serialWait == SerialPort::WAIT_TIMEOUT) {
			//cout << "serial->wait timedout" << endl;

			// don't let output buffer build up
			serial->flushIO();
		}
		else if(serialWait == SerialPort::WAIT_INT)
			cout << "serial->wait interrupted" << endl;
		else if(serialWait == SerialPort::WAIT_ERROR)
			cout << "serial->wait error" << endl;
		else if( serialWait == SerialPort::WAIT_DATA) {
			// get the command from host
			rxNum = serial->read((char *)rxBuf, CMD_MAX_SIZE);
			//cout << "clientMode() - read " << rxNum << endl;

			if( rxNum > 0) {
				serialQueue.push(rxBuf,rxNum);
				while( serialQueue.length() >= CMD_PACKET_SIZE) {

					// check the cmd and numParams bytes
					if( serialQueue.isDefined() ) {

						// start is good, do we have enough bytes
						if( !serialQueue.needMoreBytes() ) {
							if( serialQueue.isValid() ){
								Command rxCmd = serialQueue.popCommand();
								parseClientCommand(rxCmd);
							} else {
								cout << "-- Dropping byte for bad commad --" << endl
									<< (int)serialQueue.popByte() << endl;
							}
						} else
							break;
					} else {
						cout << "-- Dropping byte for bad header --" << endl
							<< (int)serialQueue.popByte() << endl;
					}
				}


			} else {
				if( rxNum == 0)
					cout << "Read 0 bytes" << endl;
				else if( errno != EWOULDBLOCK || errno != EINTR)
					perror("clientMode() - bad read from host");

				serial->close();
			}

		} else if( errno != EINTR ) {
			perror("clientMode() - serial->wait()");
		}
	} // end of for()

}

//----------------------------------------------------------
// Run like a host
//----------------------------------------------------------
void hostMode()
{
	SerialPort::SerialWait serialWait;
	Command cmd;
	uint8_t *rxBuf = new uint8_t[ CMD_MAX_SIZE ];
	CommandQueue serialQueue; // parse incoming serialet stream
	int num;

	// time command
	uint8_t cmdBuf[CMD_MAX_SIZE] = { 127 };
	cmdBuf[0] = CMD_TIME_LOOP;
	cmdBuf[1] = 0;
	Command time(cmdBuf);
	time.setCheckSum();

	printf("#################### running host mode ####################\n");
	printf("!! waiting for connection \n");

	while( hostRun )
	{
		//if( Connected == true && serial->getStatus() == SerialPort::Connected ) { 
		if( Connected == true ) {

			if(sent == false ) 
			{
				tsTx.stamp();
				if( serial->write(time) != time.size() ){
					cout << "<!!! Could not send time-loop!!!>" <<endl;
					perror ("serial->write");
					return;
				} 
				sent = true;
				//cout << "sending time-loop" << endl;
			}
		} else 
			printf("-\n");

		// wait for input, timeout at 1000 milli-sec
		serialWait = serial->wait(1000);

		if( serialWait == SerialPort::WAIT_TIMEOUT) {
			//cout << "serial->wait timedout" << endl;
			++waitCount;
			//if( waitCount % 2 ) 
				//sent = false;
			if( Connected && waitCount > 5) {
				cout << "Lost connection" << endl;
				Connected = false;
				waitCount = 0;
				serial->flushIO(); 
				sent = false;
			}
		} else if(serialWait == SerialPort::WAIT_INT)
			cout << "serial->wait interrupted" << endl;
		else if(serialWait == SerialPort::WAIT_ERROR)
			cout << "serial->wait error" << endl;
		else if( serialWait == SerialPort::WAIT_DATA) {
			waitCount=0;
			// cout << "Reading data" << endl;
			// get the command from client
			num = serial->read((char *)rxBuf, CMD_MAX_SIZE);

			if( num > 0) {
				serialQueue.push(rxBuf,num);
				while( serialQueue.length() >= CMD_PACKET_SIZE) {

					// check the cmd and numParams bytes
					if( serialQueue.isDefined() ) {

						// start is good, do we have enough bytes
						if( !serialQueue.needMoreBytes() ) {
							if( serialQueue.isValid() ){
								Command rxCmd = serialQueue.popCommand();
								parseHostCommand(rxCmd);
							} else {
								cout << "-- Dropping byte for bad csum --" << endl
									<< (int)serialQueue.popByte() << endl;
							}
						} else
							break;
					} else {
						cout << "-- Dropping byte for bad header --" << endl
							<< (int)serialQueue.popByte() << endl;
					}
				}

			} else {
				if( num == 0)
					cout << "Read 0 bytes" << endl;
				else if( errno != EWOULDBLOCK || errno != EINTR)
					perror("serverMode() - bad read from client");
			}
		}
	} // end of while
	float TotalTime = 0;
	for(size_t i=0; i < timeQueue.Used(); i++)
		TotalTime += (float)(*timeQueue.Peek(i));

	cout << "Average time = " << TotalTime / (float)(timeQueue.Used()) << "ms " << endl;
	cout << "minTime time = " << (float)(minTime / 1000.0)   << "ms " << endl;
	cout << "maxTime time = " << (float)(maxTime / 1000.0)   << "ms " << endl;

	// save data to file
	timeQueue.saveToFile("serial_time.dat" ); // save as milli-sec
}
